#include <AccelStepper.h>
#include <WiFi.h>
#include <ArduinoOTA.h>
#include <PubSubClient.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1
#define DIR_PIN  D0
#define EN_PIN   D3

#define M0_PIN   D2
#define M1_PIN   D4
#define M2_PIN   D5

#define HALL_PIN D8

// ================= WIFI =================
const char* ssid = "Ali";
const char* password = "00000000";

// ================= MQTT =================
const char* mqtt_server = "mqtt.fabcloud.org";
const int mqtt_port = 1883;
const char* mqtt_user = "fabacademy";
const char* mqtt_pass = "fabacademy";

const char* sub_topic = "fabacademy/ashtami/tool";

WiFiClient espClient;
PubSubClient client(espClient);

// ================== OTA CONTROL ==================
bool otaInProgress = false;   // blocks motion during OTA

// ================== HALL THRESHOLDS ==================
#define ON_THRESHOLD  3800
#define OFF_THRESHOLD 3400

// ================== TURNTABLE CONSTANTS ==================
#define MOTOR_STEPS   200
#define MICROSTEPS    16
#define GEAR_RATIO    5
#define STEPS_PER_REV (MOTOR_STEPS * MICROSTEPS * GEAR_RATIO)  // 16000

// ================== SPEED SETTINGS ==================
#define SEARCH_SPEED    800
#define BACKOFF_SPEED   400
#define APPROACH_SPEED  200

#define MOVE_MAX_SPEED  1200
#define MOVE_ACCEL      800
#define CRAWL_SPEED     300
#define CRAWL_THRESHOLD 200

// ================== LOOKUP TABLE ==================
// Maps (Compartment, Home) → absolute step position
const long LUT[7][6] = {
  {0,      0,      0,      0,      0,      0 },
  {0,      0,   3200,   6400,   9600,  12800 },
  {0,   2667,   5867,   9067,  12267,  15467 },
  {0,   5333,   8533,  11733,  14933,   2133 },
  {0,   8000,  11200,  14400,   1600,   4800 },
  {0,  10667,  13867,   1067,   4267,   7467 },
  {0,  13333,    533,   3733,   6933,  10133 },
};

const char* COMP_LABEL[7] = {"", "Nachos","Juice","Lays","Snickers","Popcorn","Cookies"};
const char* HOME_LABEL[6] = {"", "Abhishek","Ardra","Ali","Ashtami","Mishael"};

// ================== STEPPER ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL FILTER ==================
float filtered  = 0.0f;   // low-pass filtered analog value
bool  hallState = false;  // debounced digital state

// ================== STATE MACHINES ==================
enum HomingState { SEARCH, BACKOFF, APPROACH, HOMING_DONE };
enum SystemState { HOMING, POSITIONING, IDLE };

HomingState homingState = SEARCH;
SystemState sysState    = HOMING;

// ================== POSITION TRACKING ==================
long turntableStepPos = 0;  // logical absolute position (0–16000)
int  lastComp = 0;
int  lastHome = 0;

// ================== HALL SENSOR ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);

  // exponential smoothing to remove noise
  filtered = 0.8f * filtered + 0.2f * raw;

  // hysteresis to avoid flicker
  if (!hallState && filtered > ON_THRESHOLD) hallState = true;
  if ( hallState && filtered < OFF_THRESHOLD) hallState = false;

  return hallState;
}

// ================== HOMING FSM ==================
void runHoming() {
  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (homingState) {

    case SEARCH:
      // rotate until magnet is first detected
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {
        Serial.println("[HOMING] Magnet detected → BACKOFF");
        homingState = BACKOFF;
      }
      break;

    case BACKOFF:
      // move backward to exit magnetic zone
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();

      if (!currentHall && lastHall) {
        Serial.println("[HOMING] Magnet cleared → APPROACH");
        homingState = APPROACH;
      }
      break;

    case APPROACH:
      // slowly approach edge for precise zero
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {
        // define HOME = 0 steps
        stepper.setSpeed(0);
        stepper.setCurrentPosition(0);

        turntableStepPos = 0;
        lastComp = 1;
        lastHome = 1;

        // restore motion parameters
        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        stepper.setAcceleration(MOVE_ACCEL);

        homingState = HOMING_DONE;
        sysState    = IDLE;

        Serial.println("HOMING COMPLETE → Nachos at Abhishek");
      }
      break;

    case HOMING_DONE:
      break;
  }

  lastHall = currentHall;
}

// ================== MOVE LOGIC ==================
void moveCompartmentToHome(int c, int h) {

  // bounds check
  if (c < 1 || c > 6) { Serial.println("[ERR] C must be 1-6"); return; }
  if (h < 1 || h > 5) { Serial.println("[ERR] H must be 1-5"); return; }

  long target = LUT[c][h];
  long delta  = target - turntableStepPos;

  // shortest path normalization
  if (delta >  STEPS_PER_REV / 2) delta -= STEPS_PER_REV;
  if (delta < -STEPS_PER_REV / 2) delta += STEPS_PER_REV;

  if (delta == 0) {
    Serial.println("[INFO] Already at target");
    return;
  }

  // initiate motion
  stepper.setMaxSpeed(MOVE_MAX_SPEED);
  stepper.moveTo(stepper.currentPosition() + delta);

  sysState = POSITIONING;

  // update logical state
  turntableStepPos = target;
  lastComp = c;
  lastHome = h;

  Serial.print("Moving ");
  Serial.print(COMP_LABEL[c]);
  Serial.print(" → ");
  Serial.println(HOME_LABEL[h]);

  Serial.print("Steps: ");
  Serial.println(delta);

  Serial.print("Direction: ");
  Serial.println(delta > 0 ? "CW" : "CCW");
}

// ================== COMMAND PARSER ==================
void parseCommand(String cmd) {

  cmd.trim();
  cmd.toUpperCase();

  // CxHy format
  if (cmd.length() == 4 && cmd[0]=='C' && cmd[2]=='H') {
    int c = cmd[1] - '0';
    int h = cmd[3] - '0';
    moveCompartmentToHome(c, h);
    return;
  }

  // restart homing
  if (cmd == "HOME") {
    homingState = SEARCH;
    sysState    = HOMING;
    stepper.setMaxSpeed(SEARCH_SPEED);
    Serial.println("[HOMING] Restarting...");
    return;
  }

  // query position
  if (cmd == "POS") {
    Serial.print("Steps: ");
    Serial.println(turntableStepPos);
    return;
  }

  // OTA trigger (only if safe)
  if (cmd == "OTA") {
    if (sysState == IDLE) {
      Serial.println("[OTA] Ready for update");
      otaInProgress = true;
    } else {
      Serial.println("[OTA] Denied - System Busy");
    }
    return;
  }

  Serial.println("[ERR] Unknown command");
}

// ================== MQTT CALLBACK ==================
void callback(char* topic, byte* payload, unsigned int length) {

  String msg = "";
  for (int i = 0; i < length; i++) {
    msg += (char)payload[i];
  }

  msg.replace(":", ""); // clean formatting
  parseCommand(msg);
}

// ================== WIFI ==================
void setup_wifi() {
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  Serial.println("WiFi Connected");
}

// ================== MQTT RECONNECT ==================
void reconnect() {
  while (!client.connected()) {
    if (client.connect("ESP32S3", mqtt_user, mqtt_pass)) {
      client.subscribe(sub_topic);
    } else {
      delay(2000);
    }
  }
}

// ================== SETUP ==================
void setup() {

  Serial.begin(115200);

  // enable motor driver
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  // microstepping config
  pinMode(M0_PIN, OUTPUT); digitalWrite(M0_PIN, LOW);
  pinMode(M1_PIN, OUTPUT); digitalWrite(M1_PIN, LOW);
  pinMode(M2_PIN, OUTPUT); digitalWrite(M2_PIN, HIGH);

  // initialize hall filter
  filtered = analogRead(HALL_PIN);

  stepper.setMaxSpeed(SEARCH_SPEED);

  setup_wifi();

  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);

  // ================= OTA SETUP =================
  ArduinoOTA.setHostname("turntable-esp32s3");

  ArduinoOTA.onStart([]() {
    Serial.println("[OTA] Start");
    otaInProgress = true;
  });

  ArduinoOTA.onEnd([]() {
    Serial.println("\n[OTA] End");
  });

  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    Serial.printf("[OTA] %u%%\r", (progress * 100) / total);
  });

  ArduinoOTA.onError([](ota_error_t error) {
    Serial.printf("[OTA] Error[%u]\n", error);
  });

  ArduinoOTA.begin();
}

// ================== LOOP ==================
void loop() {

  // OTA handler must run continuously
  ArduinoOTA.handle();

  // maintain MQTT connection
  if (!client.connected()) reconnect();
  client.loop();

  // block all motion during OTA
  if (otaInProgress) return;

  switch (sysState) {

    case HOMING:
      runHoming();
      break;

    case POSITIONING:
      if (stepper.distanceToGo() != 0) {

        // slow down near target for precision
        if (abs(stepper.distanceToGo()) < CRAWL_THRESHOLD) {
          stepper.setMaxSpeed(CRAWL_SPEED);
        } else {
          stepper.setMaxSpeed(MOVE_MAX_SPEED);
        }

        stepper.run();

      } else {
        Serial.println("Move complete");
        sysState = IDLE;
      }
      break;

    case IDLE:
      if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        parseCommand(cmd);
      }
      break;
  }
}